/*
**Copyright (C) 2022, HITCRT_VISION, all rights reserved.
*/
#include "KFS.h"
namespace hitcrt {
/**
 * @description: 初始化滤波器
 * @param {double} alpha
 * @param {double} sigma2
 * @param {vector<double>} measurecov
 * @return {null}
 * @author: WWW
 * -phone:17390600977;qq:1922039181
 */
void KFS::init(const std::vector<boost::any> &param) {
    try {
        if (param.size() != 3) {
            std::string errorMessage = "init():";
            throw paramNumberError(errorMessage);
        }
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }
    alpha = boost::any_cast<double>(param[0]);
    sigma2 = boost::any_cast<double>(param[1]);
    const std::vector<double> measurecov = boost::any_cast<std::vector<double>>(param[2]);
    R0 = measurecov[0];
    m_deltaTime = 7.0 / 1000;
    m_errorCovPrior = (cv::Mat_<double>(3, 3) << R0, 0, 0, 0, R0 / m_deltaTime, 0, 0, 0,
                       R0 / m_deltaTime / m_deltaTime);
    m_errorCovPost = (cv::Mat_<double>(3, 3) << R0, 0, 0, 0, R0 / m_deltaTime, 0, 0, 0,
                      R0 / m_deltaTime / m_deltaTime);
    setTransition();
    setQ();
    m_measurement = cv::Mat::zeros(m_measureDimension, 1, CV_64F);
    cv::setIdentity(m_measurementMatrix);

    if (measurecov.size() == 1) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(measurecov[0]));
    } else if (measurecov.size() == m_measureDimension) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(1));

        for (int i = 0; i < m_measureDimension; i++) {
            m_measurementNoiseCov.at<double>(i, i) = measurecov[i];
        }
    } else {
        return;
    }
}
/**
 * @brief 设置滤波器参数
 * @param m_deltaTime:预测时间;measurecov:测量误差矩阵对角线值；measure测量数据
 * @return
 * @author www
 * -phone:17390600977;qq:1922039181
 */
void KFS::setParam(const std::vector<boost::any> &param) {
    try {
        if (param.size() != 3) {
            std::string errorMessage = "setParam():";
            throw paramNumberError(errorMessage);
        }
    } catch (const std::exception &ex) {
        std::cerr << ex.what() << std::endl;
        return;
    }
    m_deltaTime = boost::any_cast<double>(param[0]);
    const std::vector<double> measurecov = boost::any_cast<std::vector<double>>(param[1]);
    const std::vector<double> measure = boost::any_cast<std::vector<double>>(param[2]);
    if (measurecov.size() == 1) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(measurecov[0]));
    } else if (measurecov.size() == m_measureDimension) {
        setIdentity(m_measurementNoiseCov, cv::Scalar::all(1));

        for (int i = 0; i < m_measureDimension; i++) {
            m_measurementNoiseCov.at<double>(i, i) = measurecov[i];
        }
    } else {
        return;
    }
    if (measure.size() == 1) {
        setIdentity(m_measurement, cv::Scalar::all(measure[0]));
    } else {
        setIdentity(m_measurement, cv::Scalar::all(1));
        for (int i = 0; i < m_measureDimension; i++) {
            m_measurement.at<double>(i) = measure[i];
        }
    }
    setTransition();
    setQ();
}

/**
 * @description: 速度加速度无更新滤波
 * @param {null}
 * @return {null}
 * @author: WWW
 * @phone:17390600977;qq:1922039181
 */
void KFS::predictWithFixedVA() {
    //清空滤波的后验值
    m_statePost.at<double>(1) = 0;
    m_statePost.at<double>(2) = 0;
    stateUpdate();
    measureUpdate();
    //轨迹断裂，维持先验速度和加速度
    m_statePost.at<double>(1) = m_statePrior.at<double>(1);
    m_statePost.at<double>(2) = m_statePrior.at<double>(2);
}
/**
 * @description: 设置过程噪声协方差
 * @param {null}
 * @return {null}
 * @author: WWW
 * @phone:17390600977;qq:1922039181
 */
void KFS::setQ() {
    double q11 =
        (1 - exp(-2 * alpha * m_deltaTime) + 2 * alpha * m_deltaTime +
         2 * pow(alpha, 3) * pow(m_deltaTime, 3) / 3 - 2 * pow(alpha, 2) * pow(m_deltaTime, 2) -
         4 * alpha * m_deltaTime * exp(-alpha * m_deltaTime)) /
        (2 * pow(alpha, 5));
    double q12 = (1 + exp(-2 * alpha * m_deltaTime) - 2 * exp(-alpha * m_deltaTime) +
                  2 * alpha * m_deltaTime * exp(-alpha * m_deltaTime) - 2 * alpha * m_deltaTime +
                  pow(alpha, 2) * pow(m_deltaTime, 2)) /
                 (2 * pow(alpha, 4));
    double q13 =
        (1 - exp(-2 * alpha * m_deltaTime) - 2 * alpha * m_deltaTime * exp(-alpha * m_deltaTime)) /
        (2 * pow(alpha, 3));
    double q22 = (4 * exp(-alpha * m_deltaTime) - 3 - exp(-2 * alpha * m_deltaTime) +
                  2 * alpha * m_deltaTime) /
                 (2 * pow(alpha, 3));
    double q23 =
        (1 + exp(-2 * alpha * m_deltaTime) - 2 * exp(-alpha * m_deltaTime)) / (2 * pow(alpha, 2));
    double q33 = (1 - exp(-2 * alpha * m_deltaTime)) / (2 * alpha);
    ////此处与传统卡尔曼时间更新方程不同，改变过程噪声协方差计算方法
    m_processNoiseCov = 2 * alpha * sigma2 *
                        (cv::Mat_<double>(3, 3) << q11, q12, q13, q12, q22, q23, q13, q23, q33);
}
/**
 * @description: 设置状态转移矩阵
 * @param {null}
 * @return {null}
 * @author: WWW
 * @phone:17390600977;qq:1922039181
 */
void KFS::setTransition() {
    // clang-format off
    m_transitionMatrix =(cv::Mat_<double>(3, 3) << 
            1, m_deltaTime,(alpha * m_deltaTime - 1 + exp(-alpha * m_deltaTime)) / pow(alpha, 2),
            0,           1,                              (1 - exp(-alpha * m_deltaTime)) / alpha, 
            0,           0,                                            exp(-alpha * m_deltaTime));
    // clang-format on
}
/**
 * @description: 设置控制矩阵
 * @param {null}
 * @return {null}
 * @author: WWW
 * @phone:17390600977;qq:1922039181
 */
void KFS::setCon() {
    // clang-format off
    m_controlledMatrix =(cv::Mat_<double>(3, 1) << 
        (-m_deltaTime + (alpha * pow(m_deltaTime, 2)) / 2 +(1 - exp(-alpha * m_deltaTime)) / alpha) /alpha,
        m_deltaTime - (1 - exp(-alpha * m_deltaTime)) / alpha,
        1 - exp(-alpha * m_deltaTime));
    // clang-format on
}
/**
 * @description: 滤波器重置
 * @param {StateChoice} choice PRIOR设置先验，POST设置后验
 * @param {Mat} const & resetMat
 * @return {null}
 * @author: www
 * @mail: 928330305@qq.com
 */
void KFS::reset(StateChoice choice, const cv::Mat &resetMat) {
    const double deltaTime = 7.0 / 1000.0;
    //?先验?后验
    // clang-format off
    if (choice == PRIOR) {
        m_statePrior = resetMat;
        m_errorCovPost = (cv::Mat_<double>(3, 3) << 
            R0,              0,                          0, 
             0, R0 / deltaTime,                          0, 
             0,              0, R0 / deltaTime / deltaTime);
    } else {
        m_statePost = resetMat;
        m_errorCovPrior = (cv::Mat_<double>(3, 3) << 
        R0,              0,                          0, 
         0, R0 / deltaTime,                          0, 
         0,              0,R0 / deltaTime / deltaTime);
    }
    // clang-format on
}
/**
 * @description: 保存当前的先验/后验状态
 * @param {StateChoice} choice PRIOR设置先验，POST设置后验
 * @return {null}
 * @author: www
 * @mail: 928330305@qq.com
 */
void KFS::saveState(StateChoice choice) {
    if (choice == PRIOR) {
        m_stateSaved = m_statePrior;
    } else {
        m_stateSaved = m_statePost;
    }
}
/**
 * @description: 用保存的状态设置先验/后验的速度和加速度
 * @param {StateChoice} choice PRIOR设置先验，POST设置后验
 * @return {null}
 * @author: www
 * @mail: 928330305@qq.com
 */
void KFS::setVandA(StateChoice choice) {
    if (choice == PRIOR) {
        m_statePrior.at<double>(1) = m_stateSaved.at<double>(1);
        m_statePrior.at<double>(2) = m_stateSaved.at<double>(2);
    } else {
        m_statePost.at<double>(1) = m_stateSaved.at<double>(1);
        m_statePost.at<double>(2) = m_stateSaved.at<double>(2);
    }
}
/**
 * @description:只有先验更新，没有测量更新，效果相当于用匀速模型做预测，可有效改善轨迹掉目标问题
 * @param {null}
 * @return {null}
 * @author: www
 * @mail: 928330305@qq.com
 */
void KFS::statePredict() {
    setTransition();
    setQ();
    // todo ?只用先验更新,如果突然切回，后验由于无更新，导致突然造成误差
    m_statePrior = m_transitionMatrix * m_statePrior;
    m_errorCovPrior =
        m_transitionMatrix * m_errorCovPost * m_transitionMatrix.t() + m_processNoiseCov;
    m_statePost = m_statePrior;
}
} // namespace hitcrt